#ifndef FILTERING_H
#define FILTERING_H

//PCL
#include <my_point_types.h>
#include <sm2cc.h>

#include <sensor_msgs/PointCloud2.h>
#include <pcl/point_cloud.h>
#include <pcl/point_types.h>
#include <pcl/io/pcd_io.h>
#include <pcl/filters/statistical_outlier_removal.h>

int remove_outliers(sensor_msgs::PointCloud2::Ptr in_cloud, sensor_msgs::PointCloud2::Ptr out_cloud,int knn, float stddevtreshold);

struct MLSParameters
{
    enum UpsamplingMethod { NONE, SAMPLE_LOCAL_PLANE, RANDOM_UNIFORM_DENSITY, VOXEL_GRID_DILATION };

    MLSParameters():
        order_ (0),
        polynomial_fit_(false),
        search_radius_(0),
        sqr_gauss_param_(0),
        compute_normals_(false),
        upsample_method_(NONE),
        upsampling_radius_(0),
        upsampling_step_(0),
        desired_num_points_in_radius_(0)
    {
    }


    int order_;
    bool polynomial_fit_;
    double search_radius_;
    double sqr_gauss_param_;
    bool compute_normals_;
    UpsamplingMethod upsample_method_;
    double upsampling_radius_;
    double upsampling_step_;
    int desired_num_points_in_radius_;
};


/** \brief Extract SIFT keypoints
  * if only the point cloud is given PCL default parameters are used (that are not really good, so please give parameters)
  *  \note that we can pass different types as input for this function:
  * - PointXYZI
  * - PointNormal
  * - PointXYZRGB
  * \note if a PointType with a scale field is passed as output type, scales will be returned together with the return cloud
*/
template <typename PointInT, typename PointOutT> int
estimateSIFT( const typename pcl::PointCloud<PointInT>::Ptr in_cloud,
              typename pcl::PointCloud<PointOutT>::Ptr out_cloud,
              int nr_scales,
              float min_scale,
              int nr_scales_per_octave,
              float min_contrast);


template <typename PointInT, typename PointOutT> int
compute_normals(                                                const typename pcl::PointCloud<PointInT>::Ptr incloud,
                                                                const float radius,
                                                                const bool mode, //true if use knn, false if radius search
                                                                typename pcl::PointCloud<PointOutT>::Ptr outcloud
);

template <typename PointInT, typename PointOutT> int //we consider in/out types will be the same
smooth_mls(
        const typename pcl::PointCloud<PointInT>::Ptr incloud,
        const MLSParameters params,
        typename pcl::PointCloud<PointOutT>::Ptr outcloud
        );

class SAC{



};

#endif // FILTERING_H
